#include "gpio.h"
#include "comm/log.h"
Gpio::Gpio()
{
    if (!bcm2835_init())
    {
        LOG(FATAL, "bcm2835_init() FATAL");
    }
}

Gpio::~Gpio()
{
    bcm2835_close();
}

void Gpio::SetGpioMode(uint8_t pin, uint8_t model)
{
    if (model == BCM2835_GPIO_FSEL_INPT)
        bcm2835_gpio_fsel(pin, BCM2835_GPIO_FSEL_INPT);
    else
        bcm2835_gpio_fsel(pin, BCM2835_GPIO_FSEL_OUTP);

    LOG(DEBUG, "SET GPIO MODEL = %d", model);
}

void Gpio::WriteGpio(uint8_t pin, bool value)
{
    if (value)
        bcm2835_gpio_set(pin);
    else
        bcm2835_gpio_clr(pin);
}

bool Gpio::ReadGpio(uint8_t pin)
{
    return bcm2835_gpio_lev(pin) == HIGH;
}

void Gpio::SetI2cAddress(uint8_t address)
{
    bcm2835_i2c_setSlaveAddress(address);
}

void Gpio::I2cWrite(std::string &data)
{
    
    if (bcm2835_i2c_write(data.c_str(), (uint32_t)data.size()) != BCM2835_I2C_REASON_OK)
        LOG(FATAL, "I2cWrite FATAL");
}

void Gpio::I2cRead(std::string *data)
{
    if (bcm2835_i2c_read(const_cast<char*>(&(*(data->c_str()))),(uint32_t)data->size()) != BCM2835_I2C_REASON_OK)
        LOG(FATAL, "I2cRead FATAL");
}

// void Gpio::SpiWrite(std::string &data)
// {
//     if (bcm2835_spi_write(data.c_str(), (uint32_t)data.size()) != BCM2835_SPI_REASON_OK)
//         LOG(FATAL, "I2cWrite FATAL");
// }

// void Gpio::SpiRead(std::string *data)
// {
// }
